package simulator;
import java.util.*;
import java.awt.*;
import java.awt.image.*;
import javax.swing.*;
import javax.imageio.*;

import pteam.*;

import java.io.*;


/**
 * This simulates the simple control api exposed by the Roomba and Eric's code.
 * Simulator initiates all interactions in the world, and acts 
 * as the Robot's actuators.
 * Also, the Simulator allows small Actors to be attached to control 
 * robot motion/action
 */
public class Simulator extends JComponent {
	public static enum Condition { DISTANCE, ANGLE };

	public static final double TIMESTEP = 0.01; // update 100 times a second

	protected ArrayList<Point> locations = new ArrayList<Point>();
	protected ArrayList<Actor> actors = new ArrayList<Actor>();

	protected boolean acting = false;
	protected Condition condition;
	protected double[] condition_args;


	protected int speed = 100;
	protected double omega = Math.PI / 4.;
	protected double angle;
	protected double x=50, y=50;
	protected int direction = +1; // +1 = forwards, -1 = backwards, 0 = turning l/r
	protected int rot_direction = 0;  // +1 = clockwise, -1 = ccw

	public static double time = 0.0;

	public Environment env = new Environment();
	public Robot robot = new Robot();
	public static PrintWriter out = new PrintWriter(System.out);

	public void paintComponent(Graphics g) {
		if(locations.size() <= 1) {
			g.setColor(Color.RED);
			g.fillRect(100,100, 100,100);
			g.setColor(Color.BLACK);
			g.drawString("Epic... Fail.", 150, 150);
			return;
		}
		Graphics2D g2 = (Graphics2D)g;
		g2.setRenderingHint(RenderingHints.KEY_ANTIALIASING,
				RenderingHints.VALUE_ANTIALIAS_ON);

		g.setColor(Color.WHITE);
		g.fillRect(0,0,getWidth(),getHeight());
		g.setColor(Color.BLACK);

		// paint the enviornment
		for(int i=0; i<env.getObjects().size(); i++) {
			env.get(i).paint(g);
		}

		g.setColor(Color.BLACK);

		// paint lines between each location
		/*g.fillOval((int)locations.get(locations.size()-1).getX()-2,
            (int)locations.get(locations.size()-1).getY()-2,
            (int)4,
            (int)4);*/
		for(int i=0; i<locations.size()-1; i++){ 
			if(locations.size() > 1)
				g.drawLine((int)locations.get(i).getX(),
						(int)locations.get(i).getY(),
						(int)locations.get(i+1).getX(),
						(int)locations.get(i+1).getY());
		}

		// draw robot sensors
		for(int i=0; i<robot.getSensors().size(); i++) {
			robot.getSensors().get(i).paint(g);
		}
		g.setColor(Color.BLACK);

		// draw robot
		g.drawOval((int)/*locations.get(locations.size()-1).getX()*/x - 5,
				(int)/*locations.get(locations.size()-1).getY()*/y - 5,
				10, 10);

		g.setColor(Color.RED);
		// robot orientation drawn as a red line
		g.drawLine((int)locations.get(locations.size()-1).getX(),
				(int)locations.get(locations.size()-1).getY(),
				(int)(locations.get(locations.size()-1).getX() + 9 * Math.cos(angle)),
				(int)(locations.get(locations.size()-1).getY() + 9 * Math.sin(angle)));
		g.setColor(Color.BLACK);

		repaint();
	}

	/**
	 * Hands control over to the simulator until the robot completes the course
	 * or crashes in a non-recoverable fashion. (see simulationContinues);
	 * @throws RuntimeException
	 */
	public void mainLoop() {
		while (simulationContinues()) {
			if(!acting)
				robot.act(this);

			for (Actor a : actors) {
				if (! acting) {
					a.act(robot);
				}
			}
			if(acting) {
				x += speed * Math.cos(angle) * direction * TIMESTEP;
				y += speed * Math.sin(angle) * direction * TIMESTEP;
				angle += omega * rot_direction * TIMESTEP;
			}

			// physically stop any moving through walls
			// TODO: allow dragging across walls
			for(int i = 0; i<env.getObjects().size(); i++) {
				while(env.get(i).intersects(x, y)) {
					x -= speed * Math.cos(angle) * direction * TIMESTEP;
					y -= speed * Math.sin(angle) * direction * TIMESTEP;
				}
			}

			env.updateLocation(x, y, angle);

			// prepare sensor data
			Sensor[] sensors = robot.updateSensors();
			for (Actor a : actors) {
				a.updateSensors(sensors);
			}

			if (acting && condition == Condition.DISTANCE) {
				if (Math.sqrt((x-condition_args[0]) * (x-condition_args[0]) +
						(y-condition_args[1]) * (y-condition_args[1])) >=
							condition_args[2]) {
					acting = false; // distance conditions were met.
					condition_args = new double[0];
					repaint();
				}
			} else if (acting && condition == Condition.ANGLE) {
				if (condition_args[1] == 1.0?
						angle >= condition_args[0] :
							angle <= condition_args[0]) {
					acting = false; // angle conditions were met
					condition_args = new double[0];
					repaint();
				}
			}

			if (! (Math.abs(direction) + Math.abs(rot_direction) != 2)) {
				throw new RuntimeException(
						"Concurrent turning and driving is an illegal action");
			}
			if ((int)(time * (1/TIMESTEP)) % (int)(10 / TIMESTEP) == 0) {
				Simulator.out.println("time: " + (int)time);
			}
			if ((int)(time * (1/TIMESTEP)) % (int)(.1 / TIMESTEP) == 0) {
				locations.add(new Point((int)x, (int)y));
			}

			time += TIMESTEP;
		}
	}

	public boolean simulationContinues() {
		for( PhysicalObject o: env.getObjects()){
			if(o.getClass().toString().equals(Beacon.class.toString()) 
					&& Math.abs(o.getX()-x) < 5 && Math.abs(o.getY()-y ) < 5) {
				Simulator.out.println("GAME OVER");
				return false;
			}
		}
		return true;
	}

	// add an actor to the Robot
	public void addActor(Actor a) {
		actors.add(a);
	}

	public void removeActor(Actor a) {
		actors.remove(a);
	}

	public void setSpeed(int speed) {
		this.speed = speed;
	}

	public void goForward() {
		direction = +1;
		rot_direction = 0;
	}

	public void goBackward() {
		direction = -1;
		rot_direction = 0;
	}

	public void turnLeft() {
		direction = 0;
		rot_direction = -1;
	}

	public void turnRight() {
		direction = 0;
		rot_direction = +1;
	}

	/**
	 *@param degrees
	 *@condition degrees>=0
	 *After turning left or right is initated, 
	 *Continue turn until the param condtion in degrees is met.
	 *
	 */
	public void waitAngle(int degrees) {
		if(direction != 0 || rot_direction == 0) {
			throw new RuntimeException("waitAngle must be preceded by a turnLeft or "+
			"turnRight function call");
		}

		double ang = ((double)degrees) * 2 * Math.PI / 360.;
		condition = Condition.ANGLE;
		condition_args = new double[] {angle + ang * rot_direction,
				rot_direction == 1 ? 1.0 : 0.0 };
		// prevents any commands from being sent until the condition is met
		acting = true;
	}
	/**
	 *@param dist
	 *@condition dist>=0
	 *After forward/reverse motion initiated, continues motion 
	 *until param distance is achieved.
	 */
	public void waitDistance(int dist) {
		if(direction == 0 || rot_direction != 0) {
			throw new RuntimeException("waitDistance must be preceded by a goForward"+
			" or goBackward function call");
		}

		condition = Condition.DISTANCE;
		condition_args = new double[] {x, y, dist};
		// prevents any commands from being sent until the condition is met
		acting = true;
	}

	public void sampleConfiguration() {
		Simulator sim = this;
		sim.env = EnvironmentFactory.getDefaultConfiguration();
		sim.robot = RobotFactory.getDefaultConfiguration(sim.env);
	}

	public void customConfiguration(String[] args) {
		Simulator sim = this;
		if(args[0]!=null)
			try {
				sim.env = EnvironmentFactory.loadConfiguration(args[0]);
			} catch (IOException e) {
				sim.env = EnvironmentFactory.getDefaultConfiguration();
				e.printStackTrace();
			}
			else
				sim.env = EnvironmentFactory.getDefaultConfiguration();
		sim.robot = RobotFactory.getDefaultConfiguration(sim.env);
		retrieveStart();
	}

	public void setEnvironment(Environment env){
		this.env=env;
		retrieveStart();
	}

	private void retrieveStart(){
		x = env.getRobotX();
		y = env.getRobotY();
	}
	/**
	 * Save the simulator as an image to the screen.
	 */
	public void save(String path) {
		try {
			BufferedImage img = new BufferedImage(1000, 300,
					BufferedImage.TYPE_INT_ARGB);
			this.paintComponent(img.createGraphics());
			File outputFile = new File(path);
			ImageIO.write(img, "PNG", outputFile);
		} catch(Exception e) {
			e.printStackTrace(Simulator.out);
		}
	}

	public static void main(String[] args) {
		JFrame home=new JFrame("Simulator");
		
		Simulator sim = new Simulator();
		
		//ADD ACTOR(S)
		Actor b = new SolvingActor();
		sim.customConfiguration(new String[]{"maze/"+b.getMaze() +".maze"});
		sim.addActor(b);

		//This actor will serve telemetry data for Dashboard and client apps
		Actor c = new SnitchActor();
		sim.addActor(c);
		((SnitchActor) c).start();

		home.getContentPane().add(sim);
		home.setLocation(200, 200);
		home.setSize(1000, 300);
		home.setVisible(true);
		home.setDefaultCloseOperation(JFrame.EXIT_ON_CLOSE);

		//Start Simulation
		sim.mainLoop();

	}
}